#ifndef REF_STM32F4_FW_DUMMY_ROBOT_H
#define REF_STM32F4_FW_DUMMY_ROBOT_H
\
#include "algorithms/kinematic/6dof_kinematic.h"
#include "actuators/ctrl_step/ctrl_step.hpp"
#include "user_define.h"
#include<string>

#define ALL 0

/*
  |   PARAMS   | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` |
  | ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- |
  | **Joint1** | 2               | 30             | 1000     | 80       | 200      | 250      |
  | **Joint2** | 2               | 30             | 1000     | 80       | 200      | 200      |
  | **Joint3** | 2               | 30             | 1500     | 80       | 200      | 250      |
  | **Joint4** | 2               | 30             | 1000     | 80       | 200      | 250      |
  | **Joint5** | 2               | 30             | 1000     | 80       | 200      | 250      |
  | **Joint6** | 2               | 30             | 1000     | 80       | 200      | 250      |
 */


class DummyHand
        {
                public:
                uint8_t nodeID = 7;
        float maxCurrent = 0.7;
        float HandAngle = 90;
        float Velocity = 1;

        uint16_t HandOpenValue = 90;
        uint16_t HandCloeValue = 20;

        DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id);


        void SetAngle(float _angle);
        void SetMaxCurrent(float _val);
        void SetEnable(bool _enable);


        // Communication protocol definitions
        auto MakeProtocolDefinitions()
        {
            return make_protocol_member_list(
                    make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"),
                    make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"),
                    make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current")
            );
        }


        private:
        CAN_HandleTypeDef* hcan;
        uint8_t canBuf[8];
        CAN_TxHeaderTypeDef txHeader;
        float minAngle = 0;
        float maxAngle = 45;
        };


class DummyRobot
        {
                public:
                explicit DummyRobot(CAN_HandleTypeDef* _hcan);
        ~DummyRobot();


        enum CommandMode
        {
            COMMAND_TARGET_POINT_SEQUENTIAL = 1,
                    COMMAND_TARGET_POINT_INTERRUPTABLE,
                    COMMAND_CONTINUES_TRAJECTORY,
                    COMMAND_MOTOR_TUNING
        };


        class TuningHelper
        {
            public:
            explicit
            TuningHelper(DummyRobot * _context) : context(_context)
            {
            }

            void SetTuningFlag(uint8_t _flag);
            void Tick(uint32_t _timeMillis);
            void SetFreqAndAmp(float _freq, float _amp);


            // Communication protocol definitions
            auto MakeProtocolDefinitions() {
                return make_protocol_member_list(
                        make_protocol_function("set_tuning_freq_amp", *this,
                                               &TuningHelper::SetFreqAndAmp, "freq", "amp"),
                        make_protocol_function("set_tuning_flag", *this,
                                               &TuningHelper::SetTuningFlag, "flag")
                );
            }


            private:
            DummyRobot *context;
            float time = 0;
            uint8_t tuningFlag = 0;
            float frequency = 1;
            float amplitude = 1;
        };
        TuningHelper tuningHelper = TuningHelper(this);



        // This is the pose when power on.
        #ifdef DUMMY_T
            HARD_VER HarVer = DUMMY;
            uint8_t RobotAxisNum = 6;
            #if HARD_VER_ROBOT_NUM==1
                const DOF6Kinematic::Joint6D_t REST_POSE = { 175.200,-75.050,180.600,7.900,-118.800,0.000};   //DUMMY_1
            #endif
            #if HARD_VER_ROBOT_NUM==2
                const DOF6Kinematic::Joint6D_t REST_POSE = { 175.200,-75.650,180.300,4.150,-120.750,0.000};   //DUMMY_2
            #endif
            #if HARD_VER_ROBOT_NUM==3
                const DOF6Kinematic::Joint6D_t REST_POSE = { 175.200,-75.850,180.500,1.000,-120.850,0.000};   //DUMMY_3
            #endif

            const DOF6Kinematic::Joint6D_t SHUTUP_POSE = {0,0,90,0,0,0};

            /***************需要存flash****************************/
            DOF6Kinematic::Joint6D_t ADJUST_ANGLE_POSITION = { 0,0,90,0,0,0  };
            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION = { 0,0,0,0,0,0 };


            #if HARD_VER_ROBOT_NUM==1
                DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION_ERR = { 0,0,  -19.40, -2.15, 9.13,0 };     //DUMMY_1
            #endif
            #if HARD_VER_ROBOT_NUM==3
                DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION_ERR = { 0,0,  -15.72, -7.00, -10.16, -3.98 };     //DUMMY_3
            #endif

            bool HAVE_PHYSICAL_LIMIT[8] = {true,true,true,true,true,true,false,false};
            bool PHYSICAL_LIMIT_DIRT[8] = {false,true,false,true,true,false,false,false};
            uint16_t FIND_PHYSICAL_LIMIT_MAX_CURRENT[8] = {0,500,500,500,500,1500,500,0};

            bool HAVE_KEY_DETECT[8] = {true,true,true,true,true,true,true,false};
            bool KEY_DETECT_DIRT[8] = {true,true,false,true,false,true,true,false};
            uint16_t FIND_KEY_MAX_CURRENT[8] = {0,600,600,600,500,500,500,0};

            NEED_FIND_HOME_FLAG IS_NEED_FIND_HOME[8] = {IS_NOT_NEED,FIND_HOME_WITH_PHYSICAL_LIMIT,FIND_HOME_WITH_PHYSICAL_LIMIT,FIND_HOME_WITH_PHYSICAL_LIMIT,FIND_HOME_WITH_PHYSICAL_LIMIT,FIND_HOME_WITH_PHYSICAL_LIMIT,IS_NOT_NEED,IS_NOT_NEED};

            uint16_t FIND_HOME_ERR_PER = 500;

            const float DEFAULT_JOINT_SPEED = 30;  // degree/s
            const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = { 150, 100, 200, 200, 200, 200 };
            const float DEFAULT_JOINT_ACCELERATION_LOW = 30;    // 0~100
            const float DEFAULT_JOINT_ACCELERATION_HIGH = 100;  // 0~100
            const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;

            DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
            DOF6Kinematic::Joint6D_t targetJoints = SHUTUP_POSE;
            DOF6Kinematic::Joint6D_t initPose = REST_POSE;
            DOF6Kinematic::Joint6D_t ShutupPose = SHUTUP_POSE;
            /***************************************************/
        #endif
        #ifdef AR3_T
            HARD_VER HarVer = AR3;
            uint8_t RobotAxisNum = 6;
            const DOF6Kinematic::Joint6D_t REST_POSE = { 160,-41.5,147.800,-58.400,95.000,163.100};   //AR3
            const DOF6Kinematic::Joint6D_t SHUTUP_POSE = {0,0,90,0,0,0};
            /***************需要存flash****************************/
            DOF6Kinematic::Joint6D_t ADJUST_ANGLE_POSITION = { 0,0,90,0,0,0  };
            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION = { 0,0,0,0,0,0 };


            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION_ERR = { 0,0,  -19.40, -2.15, 9.13,0 };

            bool HAVE_PHYSICAL_LIMIT[8] = {true,true,true,true,true,true,false,false};
            bool PHYSICAL_LIMIT_DIRT[8] = {false,true,false,true,true,false,false,false};
            uint16_t FIND_PHYSICAL_LIMIT_MAX_CURRENT[8] = {0,1000,1000,1000,500,500,500,0};

            bool HAVE_KEY_DETECT[8] = {true,true,true,true,true,true,true,false};
            bool KEY_DETECT_DIRT[8] = {true,true,false,true,false,true,true,false};
            uint16_t FIND_KEY_MAX_CURRENT[8] = {0,1000,1000,1000,500,500,500,0};

            const float DEFAULT_JOINT_SPEED = 30;  // degree/s
            const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = { 150, 100, 200, 200, 200, 200 };
            const float DEFAULT_JOINT_ACCELERATION_LOW = 30;    // 0~100
            const float DEFAULT_JOINT_ACCELERATION_HIGH = 100;  // 0~100
            const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;


            DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
            DOF6Kinematic::Joint6D_t targetJoints = SHUTUP_POSE;
            DOF6Kinematic::Joint6D_t initPose = REST_POSE;
            DOF6Kinematic::Joint6D_t ShutupPose = SHUTUP_POSE;
            /***************************************************/
        #endif
        #ifdef SITO_T
            HARD_VER HarVer = SITO;
            uint8_t RobotAxisNum = 6;
            const DOF6Kinematic::Joint6D_t REST_POSE = { 0,0,0,0,0,0};   //AR3
            const DOF6Kinematic::Joint6D_t SHUTUP_POSE = {0,0,0,0,0,0};

            /***************需要存flash****************************/
            DOF6Kinematic::Joint6D_t ADJUST_ANGLE_POSITION = { 0,0,90,0,0,0  };
            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION = { 0,0,0,0,0,0 };


            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION_ERR = { 0,0,  -19.40, -2.15, 9.13,0 };

            bool HAVE_PHYSICAL_LIMIT[8] = {true,true,true,true,true,true,false,false};
            bool PHYSICAL_LIMIT_DIRT[8] = {false,true,false,true,true,false,false,false};
            uint16_t FIND_PHYSICAL_LIMIT_MAX_CURRENT[8] = {0,1000,1000,1000,500,500,500,0};

            bool HAVE_KEY_DETECT[8] = {true,true,true,true,true,true,true,false};
            bool KEY_DETECT_DIRT[8] = {true,true,false,true,false,true,true,false};
            uint16_t FIND_KEY_MAX_CURRENT[8] = {0,1000,1000,1000,500,500,500,0};

            const float DEFAULT_JOINT_SPEED = 30;  // degree/s
            const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = { 150, 100, 200, 200, 200, 200 };
            const float DEFAULT_JOINT_ACCELERATION_LOW = 30;    // 0~100
            const float DEFAULT_JOINT_ACCELERATION_HIGH = 100;  // 0~100
            const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;


            DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
            DOF6Kinematic::Joint6D_t targetJoints = SHUTUP_POSE;
            DOF6Kinematic::Joint6D_t initPose = REST_POSE;
            DOF6Kinematic::Joint6D_t ShutupPose = SHUTUP_POSE;
            /***************************************************/
        #endif
        #ifdef BATTLE_CHARGE_THERE_AXIS_T
            HARD_VER HarVer = BATTLE_CHARGE_THERE_AXIS;
            uint8_t RobotAxisNum = 4;
            const float PosXMaxAbout = 2;
            const float PosYMaxAbout = 2;
            const float PosZMaxAbout = 2;
            #if HARD_VER_ROBOT_NUM==0
                const DOF6Kinematic::Joint6D_t REST_POSE = { 0,0,0,0.000,0,0};
            #endif
            #if HARD_VER_ROBOT_NUM==1
                const DOF6Kinematic::Joint6D_t REST_POSE = { 0,0,0,-0.0012,0,0};
            #endif
            const DOF6Kinematic::Joint6D_t SHUTUP_POSE = {0.5,0.5,0,0.5,0,0};

            /***************需要存flash****************************/
            DOF6Kinematic::Joint6D_t ADJUST_ANGLE_POSITION = { 0,0,90,0,0,0  };
            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION = { 0,0,0,0,0,0 };


            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION_ERR = { 0,0,  -19.40, -2.15, 9.13,0 };

            bool HAVE_PHYSICAL_LIMIT[8] = {true,true,true,true,true,true,false,false};
            bool PHYSICAL_LIMIT_DIRT[8] = {false,true,false,true,true,false,false,false};
            uint16_t FIND_PHYSICAL_LIMIT_MAX_CURRENT[8] = {0,1000,1000,1000,1000,1000,1000,0};

            bool HAVE_KEY_DETECT[8] = {true,true,true,true,true,true,true,false};
            bool KEY_DETECT_DIRT[8] = {false, false,false,true,false,true,true,false};
            uint16_t FIND_KEY_MAX_CURRENT[8] = {0,2000,2000,2000,2000,1000,1000,0};

            NEED_FIND_HOME_FLAG IS_NEED_FIND_HOME[8] = {IS_NOT_NEED,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,IS_NOT_NEED,IS_NOT_NEED,IS_NOT_NEED};

            uint16_t FIND_HOME_ERR_PER = 40;

            const float DEFAULT_JOINT_SPEED = 30;  // degree/s
            const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = { 150, 100, 200, 200, 200, 200 };
            const float DEFAULT_JOINT_ACCELERATION_LOW = 30;    // 0~100
            const float DEFAULT_JOINT_ACCELERATION_HIGH = 100;  // 0~100
            const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;


            DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
            DOF6Kinematic::Joint6D_t targetJoints = SHUTUP_POSE;
            DOF6Kinematic::Joint6D_t initPose = REST_POSE;
            DOF6Kinematic::Joint6D_t ShutupPose = SHUTUP_POSE;
            /***************************************************/

        #endif


        #ifdef IMU6_T
            HARD_VER HarVer = IMU6;
            uint8_t RobotAxisNum = 4;
            const float PosXMaxAbout = 2;
            const float PosYMaxAbout = 2;
            const float PosZMaxAbout = 2;
            #if HARD_VER_ROBOT_NUM==0
                const DOF6Kinematic::Joint6D_t REST_POSE = { 0,0,0,0.000,0,0};
            #endif
            #if HARD_VER_ROBOT_NUM==1
                const DOF6Kinematic::Joint6D_t REST_POSE = { 0,0,0,-0.0012,0,0};
            #endif
            const DOF6Kinematic::Joint6D_t SHUTUP_POSE = {0.5,0.5,0,0.5,0,0};

            /***************需要存flash****************************/
            DOF6Kinematic::Joint6D_t ADJUST_ANGLE_POSITION = { 0,0,90,0,0,0  };
            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION = { 0,0,0,0,0,0 };


            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION_ERR = { 0,0,  -19.40, -2.15, 9.13,0 };

            bool HAVE_PHYSICAL_LIMIT[8] = {true,true,true,true,true,true,false,false};
            bool PHYSICAL_LIMIT_DIRT[8] = {false,true,false,true,true,false,false,false};
            uint16_t FIND_PHYSICAL_LIMIT_MAX_CURRENT[8] = {0,1000,1000,1000,1000,1000,1000,0};

            bool HAVE_KEY_DETECT[8] = {true,true,true,true,true,true,true,false};
            bool KEY_DETECT_DIRT[8] = {false, false,false,true,false,true,true,false};
            uint16_t FIND_KEY_MAX_CURRENT[8] = {0,2000,2000,2000,2000,1000,1000,0};

            NEED_FIND_HOME_FLAG IS_NEED_FIND_HOME[8] = {IS_NOT_NEED,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,IS_NOT_NEED,IS_NOT_NEED,IS_NOT_NEED};

            uint16_t FIND_HOME_ERR_PER = 40;

            const float DEFAULT_JOINT_SPEED = 30;  // degree/s
            const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = { 150, 100, 200, 200, 200, 200 };
            const float DEFAULT_JOINT_ACCELERATION_LOW = 30;    // 0~100
            const float DEFAULT_JOINT_ACCELERATION_HIGH = 100;  // 0~100
            const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;


            DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
            DOF6Kinematic::Joint6D_t targetJoints = SHUTUP_POSE;
            DOF6Kinematic::Joint6D_t initPose = REST_POSE;
            DOF6Kinematic::Joint6D_t ShutupPose = SHUTUP_POSE;
            /***************************************************/

        #endif

        #ifdef MR6_T
            HARD_VER HarVer = MR6;
            uint8_t RobotAxisNum = 6;
            const float PosXMaxAbout = 1000;
            const float PosYMaxAbout = 1000;
            const float PosZMaxAbout = 1000;
            #if HARD_VER_ROBOT_NUM == 1
                const DOF6Kinematic::Joint6D_t REST_POSE = { 168.700,-56.170,153.800,-155.52,66.64285,33.200}; //1号
            #endif
            #if HARD_VER_ROBOT_NUM == 2
                const DOF6Kinematic::Joint6D_t REST_POSE = { 136.200,-35.270,149.100,-192.838,81.692,-127.809};  //2号
            #endif
            #if HARD_VER_ROBOT_NUM == 3
                const DOF6Kinematic::Joint6D_t REST_POSE = { 119.900,-35.370,135.800,-12.070,92.500,-133.509};     //3号
            #endif
            #if HARD_VER_ROBOT_NUM == 4
                const DOF6Kinematic::Joint6D_t REST_POSE = { -7.000,-36.870,137.700,-7.9875,88.6125,193.300};  //4号
            #endif

            const DOF6Kinematic::Joint6D_t SHUTUP_POSE = {0,0,90,0,0,0};

            /***************需要存flash****************************/
            DOF6Kinematic::Joint6D_t ADJUST_ANGLE_POSITION = { 0,0,90,0,0,0  };
            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION = { 0,0,0,0,0,0 };


            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION_ERR = { 0,0,  -19.40, -2.15, 9.13,0 };

            bool HAVE_PHYSICAL_LIMIT[8] = {true,true,true,true,true,true,false,false};
            bool PHYSICAL_LIMIT_DIRT[8] = {false,true,false,true,true,false,false,false};
            uint16_t FIND_PHYSICAL_LIMIT_MAX_CURRENT[8] = {0,1000,1000,1000,1000,1000,1000,0};

            bool HAVE_KEY_DETECT[8] = {true,true,true,true,true,true,true,false};

            #if HARD_VER_ROBOT_NUM == 1
                bool KEY_DETECT_DIRT[8] = {true, true,false,true,false,true,true,false};    //1号
            #endif
            #if HARD_VER_ROBOT_NUM == 2
                bool KEY_DETECT_DIRT[8] = {true, true,false,true,true,true,false,false};   //2号
            #endif
            #if HARD_VER_ROBOT_NUM == 3
                bool KEY_DETECT_DIRT[8] = {true, true,false,true,true,true,false,false}; //3号
            #endif
            #if HARD_VER_ROBOT_NUM == 4
                bool KEY_DETECT_DIRT[8] = {true, false,false,true,true,true,true,false}; //4号
            #endif


            uint16_t FIND_KEY_MAX_CURRENT[8] = {0,1000,2000,2000,1000,1000,1000,0};

            NEED_FIND_HOME_FLAG IS_NEED_FIND_HOME[8] = {IS_NOT_NEED,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,IS_NOT_NEED};

            uint16_t FIND_HOME_ERR_PER = 1000;

            const float DEFAULT_JOINT_SPEED = 30;  // degree/s
            const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = { 150, 100, 200, 200, 200, 200 };
            const float DEFAULT_JOINT_ACCELERATION_LOW = 30;    // 0~100
            const float DEFAULT_JOINT_ACCELERATION_HIGH = 100;  // 0~100
            const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;


            DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
            DOF6Kinematic::Joint6D_t targetJoints = SHUTUP_POSE;
            DOF6Kinematic::Joint6D_t initPose = REST_POSE;
            DOF6Kinematic::Joint6D_t ShutupPose = SHUTUP_POSE;
            /***************************************************/
        #endif

        #if defined(IRC_T)
            HARD_VER HarVer = IRC;
            uint8_t RobotAxisNum = 6;
            const float PosXMaxAbout = 1000;
            const float PosYMaxAbout = 1000;
            const float PosZMaxAbout = 1000;
            #if HARD_VER_ROBOT_NUM == 1
                const DOF6Kinematic::Joint6D_t REST_POSE = { 168.700,-56.170,153.800,-155.52,66.64285,33.200}; //1号
            #endif
            #if HARD_VER_ROBOT_NUM == 2
                const DOF6Kinematic::Joint6D_t REST_POSE = { 136.200,-35.270,149.100,-192.838,81.692,-127.809};  //2号
            #endif
            #if HARD_VER_ROBOT_NUM == 3
                const DOF6Kinematic::Joint6D_t REST_POSE = { 119.900,-35.370,135.800,-12.070,92.500,-133.509};     //3号
            #endif
            #if HARD_VER_ROBOT_NUM == 4
                const DOF6Kinematic::Joint6D_t REST_POSE = { -7.000,-36.870,137.700,-7.9875,88.6125,193.300};  //4号
            #endif

            const DOF6Kinematic::Joint6D_t SHUTUP_POSE = {0,0,90,0,0,0};

            /***************需要存flash****************************/
            DOF6Kinematic::Joint6D_t ADJUST_ANGLE_POSITION = { 0,0,90,0,0,0  };
            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION = { 0,0,0,0,0,0 };


            DOF6Kinematic::Joint6D_t ADJUST_IMU_POSITION_ERR = { 0,0,  -19.40, -2.15, 9.13,0 };

            bool HAVE_PHYSICAL_LIMIT[8] = {true,true,true,true,true,true,false,false};
            bool PHYSICAL_LIMIT_DIRT[8] = {false,true,false,true,true,false,false,false};
            uint16_t FIND_PHYSICAL_LIMIT_MAX_CURRENT[8] = {0,1000,1000,1000,1000,1000,1000,0};

            bool HAVE_KEY_DETECT[8] = {true,true,true,true,true,true,true,false};

            #if HARD_VER_ROBOT_NUM == 1
                bool KEY_DETECT_DIRT[8] = {true, true,false,true,false,true,true,false};    //1号
            #endif
            #if HARD_VER_ROBOT_NUM == 2
                bool KEY_DETECT_DIRT[8] = {true, true,false,true,true,true,false,false};   //2号
            #endif
            #if HARD_VER_ROBOT_NUM == 3
                bool KEY_DETECT_DIRT[8] = {true, true,false,true,true,true,false,false}; //3号
            #endif
            #if HARD_VER_ROBOT_NUM == 4
                bool KEY_DETECT_DIRT[8] = {true, false,false,true,true,true,true,false}; //4号
            #endif


            uint16_t FIND_KEY_MAX_CURRENT[8] = {0,1000,2000,2000,1000,1000,1000,0};

            NEED_FIND_HOME_FLAG IS_NEED_FIND_HOME[8] = {IS_NOT_NEED,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,FIND_HOME_WITH_PHYSICAL_KEY,IS_NOT_NEED};

            uint16_t FIND_HOME_ERR_PER = 1000;

            const float DEFAULT_JOINT_SPEED = 30;  // degree/s
            const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = { 150, 100, 200, 200, 200, 200 };
            const float DEFAULT_JOINT_ACCELERATION_LOW = 30;    // 0~100
            const float DEFAULT_JOINT_ACCELERATION_HIGH = 100;  // 0~100
            const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;


            DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
            DOF6Kinematic::Joint6D_t targetJoints = SHUTUP_POSE;
            DOF6Kinematic::Joint6D_t initPose = REST_POSE;
            DOF6Kinematic::Joint6D_t ShutupPose = SHUTUP_POSE;
            /***************************************************/

        #endif

        uint8_t HardVerSeriesNum = HARD_VER_SERIES_NUM;
        uint8_t HardVerRobotNum = HARD_VER_ROBOT_NUM;

        DOF6Kinematic::Pose6D_t currentPose6D = {};
        DOF6Kinematic::Pose6D_t targetPose6D = {};
        volatile uint8_t jointsStateFlag = 0b00000000;
        volatile uint8_t jointsEnableFlag = 0b00000000;
        CommandMode commandMode = DEFAULT_COMMAND_MODE;
        CtrlStepMotor* motorJ[8] = { nullptr };
        DummyHand* hand = { nullptr };

        float SpeedLimit = DEFAULT_JOINT_SPEED;



        void Init();
        bool MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6);
        bool MoveL(float _x, float _y, float _z, float _a, float _b, float _c);
        void MoveJoints(DOF6Kinematic::Joint6D_t _joints);
        void SetJointSpeed(float _speed);
        void SetJointAcceleration(float _acc);
        void UpdateJointAngles();
        void UpdateJointAnglesCallback();
        void UpdateJointPose6D();
        void Reboot();
        void SetEnable(bool _enable);
        void CalibrateHomeOffset();
        void Homing();
        void Resting();
        bool IsMoving();
        bool IsAllJointsEnable();
        bool IsArrived(float err);
        bool IsPosArrived(float XErrPermit,float YErrPermit,float ZErrPermit,float AErrPermit,float BErrPermit,float CErrPermit);
        float DisOfTwoPos(float Ax,float Ay,float Az,float Bx,float By,float Bz);

    bool IsEnabled();
        void SetCommandMode(uint32_t _mode);


        // Communication protocol definitions
        auto MakeProtocolDefinitions()
        {
            return make_protocol_member_list(
                    make_protocol_function("calibrate_home_offset", *this, &DummyRobot::CalibrateHomeOffset),
                    make_protocol_function("homing", *this, &DummyRobot::Homing),
                    make_protocol_function("resting", *this, &DummyRobot::Resting),
                    make_protocol_object("joint_1", motorJ[1]->MakeProtocolDefinitions()),
                    make_protocol_object("joint_2", motorJ[2]->MakeProtocolDefinitions()),
                    make_protocol_object("joint_3", motorJ[3]->MakeProtocolDefinitions()),
                    make_protocol_object("joint_4", motorJ[4]->MakeProtocolDefinitions()),
                    make_protocol_object("joint_5", motorJ[5]->MakeProtocolDefinitions()),
                    make_protocol_object("joint_6", motorJ[6]->MakeProtocolDefinitions()),
                    make_protocol_object("joint_all", motorJ[ALL]->MakeProtocolDefinitions()),
                    make_protocol_object("hand", hand->MakeProtocolDefinitions()),
                    make_protocol_function("reboot", *this, &DummyRobot::Reboot),
                    make_protocol_function("set_enable", *this, &DummyRobot::SetEnable, "enable"),
                    make_protocol_function("move_j", *this, &DummyRobot::MoveJ, "j1", "j2", "j3", "j4", "j5", "j6"),
                    make_protocol_function("move_l", *this, &DummyRobot::MoveL, "x", "y", "z", "a", "b", "c"),
                    make_protocol_function("set_joint_speed", *this, &DummyRobot::SetJointSpeed, "speed"),
                    make_protocol_function("set_joint_acc", *this, &DummyRobot::SetJointAcceleration, "acc"),
                    make_protocol_function("set_command_mode", *this, &DummyRobot::SetCommandMode, "mode"),
                    make_protocol_object("tuning", tuningHelper.MakeProtocolDefinitions())
            );
        }


        class CommandHandler
        {
            public:
            explicit
            CommandHandler(DummyRobot * _context) : context(_context)
            {
                commandFifo = osMessageQueueNew(16, 64, nullptr);
            }

            uint32_t
            Push(
            const std::string
            &_cmd);
            std::string Pop(uint32_t timeout);
            uint32_t
            ParseCommand(
            const std::string
            &_cmd);
            uint32_t GetSpace();
            void ClearFifo();
            void EmergencyStop();


            private:
            DummyRobot *context;
            osMessageQueueId_t commandFifo;
            char strBuffer[64]{};
        };
        CommandHandler commandHandler = CommandHandler(this);

    DOF6Kinematic::Joint6D_t dynamicJointSpeeds = { 1, 1, 1, 1, 1, 1 };
    DOF6Kinematic* dof6Solver;
private:
        CAN_HandleTypeDef* hcan;
        float jointSpeed = DEFAULT_JOINT_SPEED;
        float jointSpeedRatio = 1;

    bool isEnabled = false;
        };


#endif //REF_STM32F4_FW_DUMMY_ROBOT_H
